#include "kalman.h"
#include <math.h>
/*传入数据Data_a[],Data_b[],Data_c[]分别为三个关于锚点1，2，3的距离数组
X_P,Y_P分别为返回的x，y坐标*/
void position(int Data_a[], int Data_b[], int Data_c[], double *X_P, double *Y_P)
{
	static int flag = 0;//函数调用标志
	static const int Anchor_X[3] = { 0,-400,400 };//锚点的x坐标
	static const int Anchor_Y[3] = { 0,450,230 };//锚点的y坐标
	int D1, D2, D3;//待测点到每个锚点的距离
	double M[4],N[2],T[4],P[2],r,t;
	double X[4];//坐标先验估计矩阵
	static double Y[4];//坐标后验估计矩阵
	const double A[4][4] = { {1,1,0,0},{0,1,0,0},{0,0,1,1},{ 0,0,0,1 } };//当前坐标预测矩阵
	const double Qk[4][4] = { {10,0,0,0},{0,5,0,0},{ 0,0,10,0 },{0,0,0,5} };//过程激励噪声矩阵
	const double I[4][4] = { { 1,0,0,0 },{ 0,1,0,0 },{ 0,0,1,0 },{ 0,0,0,1 } };//单位矩阵
	const double H[2][4] = { { 1,0,0,0 },{ 0,0,1,0 } };//
	const double Rk[2][2] = { { 10,0 },{ 0,10 } };//观测噪声矩阵
	static double P1[4][4]= { { 10,0,0,0 },{ 0,5,0,0 },{ 0,0,10,0 },{ 0,0,0,5 } };//方差估计
	double P2[4][4], Temp[4][4], Kk[4][2], Temp1[2][2], Temp2[4][2];

	//将串口传入的信号转换为到每个锚点的距离值，单位为cm
	if ((*Data_a == 0xe0) && (*(Data_a + 1) == 0x00))
	{
		D1 = (*(Data_a + 3) * 65536 + *(Data_a + 4) * 256 + *(Data_a + 5)) * 75000 / (128 * 4992);
	}
	if ((*Data_b == 0xe0) & (*(Data_b + 1) == 0x01))
	{
		D2 = (*(Data_b + 3) * 65536 + *(Data_b + 4) * 256 + *(Data_b + 5)) * 75000 / (128 * 4992);
	}
	if ((*Data_c == 0xe0) & (*(Data_c + 1) == 0x02))
	{
		D3 = (*(Data_c + 3) * 65536 + *(Data_c + 4) * 256 + *(Data_c + 5)) * 75000 / (128 * 4992);
	}
	//通过极大似然估计计算坐标
	M[0] = 2 * (Anchor_X[0] - Anchor_X[2]);
	M[2] = 2 * (Anchor_Y[0] - Anchor_Y[2]);
	M[1] = 2 * (Anchor_X[1] - Anchor_X[2]);
	M[3] = 2 * (Anchor_Y[1] - Anchor_Y[2]);

	N[0] = (pow(Anchor_X[0] , 2) - pow(Anchor_X[2] ,2) + pow(Anchor_Y[0],2)- pow(Anchor_Y[2] , 2) + pow(D3 , 2) - pow(D1 ,2));
	N[1] = (pow(Anchor_X[1], 2) - pow(Anchor_X[2], 2) + pow(Anchor_Y[1], 2) - pow(Anchor_Y[2], 2) + pow(D3, 2) - pow(D2, 2));

	//T=M的转置
	if (fabs(M[1]) > fabs(M[0]))
	{
		r = M[0] / M[1];
		t = 1.0 / (r * M[3] - M[2]);
		T[0] = M[3] / M[1] * t;
		T[1] = -t;
		T[2] = -M[2] / M[1] * t;
		T[3] = r * t;
	}
	else
	{
		r = M[1] / M[0];
		t = 1.0 / (M[3] - r * M[2]);
		T[0] = M[3] / M[0] * t;
		T[1] = -r * t;
		T[2] = -M[2] / M[0] * t;
		T[3] = t;
	}
	//P为最大似然估计解算出的位置坐标
	P[0] = T[0] * N[0] + T[2] * N[1];
	P[1] = T[1] * N[0] + T[3] * N[1];
	//赋初值
	if (flag == 0)
	{
		Y[0] = P[0];
		Y[2] = P[1];
	}

	//Y=A*Y,当前时刻状态的预测值
	for (int i = 0; i < 4; i++)
	{
		double sum=0;
		for (int j = 0; j < 4; j++)
		{
			sum = sum + A[i][j] * Y[j];
		}
		X[i] = sum;
	}

	//P2=A*P1*A'+Qk,当前时刻状态的预测值的方差矩阵
	for (int i = 0; i < 4 ; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			double sum = 0;
			for (int k = 0; k < 4; k++)
			{
				sum = sum + A[i][k] * P1[k][j];
			}
			Temp[i][j] = sum;
		}
	}
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			double sum = 0;
			for (int k = 0; k < 4; k++)
			{
				sum = sum + Temp[i][k] * A[j][k];
			}
			P2[i][j] = sum+Qk[i][j];
		}
	}


	//卡尔曼增益Kk = P2*H'/(H*P2*H'+Rk)
	for (int i = 0; i < 2; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			double sum = 0;
			for (int k = 0; k < 4; k++)
			{
				sum = sum + H[i][k] * P2[k][j];
			}
			Temp[i][j] = sum;
		}
	}
	for (int i = 0; i < 2; i++)
	{
		for (int j = 0; j < 2; j++)
		{
			double sum = 0;
			for (int k = 0; k < 4; k++)
			{
				sum = sum + Temp[i][k] * H[j][k];
			}
			Temp1[i][j] = sum + Rk[i][j];//(H*P2*H'+Rk)
		}
	}
	double temp, temp1;//数据暂存
	temp = Temp1[0][0]* Temp1[1][1]- Temp1[0][1]* Temp1[1][1];//行列式

	temp1 = Temp1[0][0] / temp; //得到逆矩阵
	Temp1[0][0] = Temp1[1][1] / temp;
	Temp1[1][1] = temp1;
	temp1 = -Temp1[1][0] / temp;
	Temp1[0][1] = -Temp1[0][1] / temp;
	Temp1[1][0] = temp1;

	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 2; j++)
		{
			double sum = 0;
			for (int k = 0; k < 4; k++)
			{
				sum = sum + (P2[i][k] * H[j][k]);
			}
			Temp2[i][j] = sum;
		}
	}
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 2; j++)
		{
			double sum = 0;
			for (int k = 0; k < 2; k++)
			{
				sum = sum + (Temp2[i][k] * Temp1[k][j]);
			}
			Kk[i][j] = sum;//kalman增益
		}
	}

	//当前状态的估计值
	P[0] = P[0] - H[0][0] * X[0] - H[0][1] * X[1] - H[0][2] * X[2] - H[0][3] * X[3];
	P[1] = P[1] - H[1][0] * X[0] - H[1][1] * X[1] - H[1][2] * X[2] - H[1][3] * X[3];
	for (int i = 0; i < 4; i++)
	{
		Y[i] = Y[i] + Kk[i][0] * P[0] + Kk[i][1] * P[1];
	}
	
	//当前时刻状态的估计值的方差矩阵
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			Temp[i][j] = I[i][j] - Kk[i][0] * H[0][j] - Kk[i][1] * H[1][j];
		}
	}
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			double sum = 0;
			for (int k = 0; k < 4; k++)
			{
				sum = sum + Temp[i][k] * P2[k][j];
			}
			P1[i][j] = sum;
		}

	}
	flag = 1;//将运行次数标志位置为1，表示程序已经运行过一次
	*X_P = Y[0];
	*Y_P = Y[2];
}

